/*
 * Copyright 2011 Maikel Nadolski. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are
 * permitted provided that the following conditions are met:
 *
 *   1. Redistributions of source code must retain the above copyright notice, this list of
 *      conditions and the following disclaimer.
 *
 *   2. Redistributions in binary form must reproduce the above copyright notice, this list
 *      of conditions and the following disclaimer in the documentation and/or other materials
 *      provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY Maikel Nadolski ``AS IS'' AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MAIKEL NADOLSKI OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
/*
 * email: maikel.nadolski@googlemail.com
 */
 
#include <assert.h>
#include <stdlib.h> 
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <errno.h>

/* setting the map height and width */
#define MAP_WIDTH 2002
#define MAP_HEIGHT 2002

/* converting x and y coordinates into an array position */
#define pos(x,y) (x + y * height)

/* calculate x and y coordinates from position in the array */
#define x(p) (p % width)
#define y(p) (p / height)

/* relations on the map */
#define N(x) (x - height) /* NORTH */
#define S(x) (x + height) /* SOUTH */
#define E(x) (x + 1)      /* EAST  */
#define W(x) (x - 1)      /* WEST  */

/* calculate relations in the heap */
#define parent(x) ((x-1)/2)
#define left(x)   (2*x + 1)
#define right(x)  (2*x + 2)

/* useful defines */
#define max(x,y) (x > y ? x : y)
#define is_in_map(p) (x1 <= x(p) && x(p) <= x2 && y1 <= y(p) && y(p) <= y2)

/*****************************************************************************
 *                            the map                                        *
 *****************************************************************************/

/* this combines map_get with map_weight */
int map_get_weight(const char *map, int index) {
    switch (map[index]) {
        case '1': return 4;
        case '2': return 5;
        case '3': return 6;
        case '4': return 7;
        case '5': return 10;
        case '6': return 8;
        case '7': return 12;
        case '8': return 24;
        case '9': return 255;
    }
    return 0;
}

/*
 * reads the csv from andaloria and returns a new map char array (not string!).
 * By returning the NULL pointer this function indicates an error.
 * The error can rise either from IO or by failing to allocate memory.
 */
char *map_from_csv(const char *filename) {
    /* open file and look, if there are any problems ... */
    FILE *file = fopen(filename, "r");
    /* try to allocate a map */
    char *map = calloc(MAP_WIDTH * MAP_HEIGHT, sizeof(char));

    if (map == NULL || file == NULL)
        return NULL;

    /* fill the Map structure */
    int len;
    int bytes = 0;
    char buffer[2048];
    while(fgets(buffer, sizeof(buffer), file) != NULL) {
        len = strlen(buffer);
        /* do not copy '\n' */
        if (buffer[len-1] == '\n')
            len = len - 1;
        strncpy(map + bytes, buffer, len); 
        bytes += len;
    }
    return map;
}

/*****************************************************************************
 *                        the list datastructure                             *
 *****************************************************************************/


/* 
 * simply linked list
 */
typedef struct _list {
    struct _list *next;
    int data;
} List;

/* 
 * Delete the complete list.
 */
void list_delete(List *list) {
    List *node = list;
    while (node != NULL) {
        List *next = node->next;
        free(node);
        node = next;
    }
}

/*
 * Add data into a list. If list is NULL, then a new list is created.
 * The data will be on the head of the list.
 */
List *list_add(List *list, int data) {
    List *node = malloc(sizeof(List));
    node->data = data;
    node->next = list;
    
    return node;
}

/*****************************************************************************
 *            declare functions, which will be used in A*                    *
 *****************************************************************************/

/*
 * Reconstruct the path from start to goal and return the list of all nodes
 * laying on the path.
 */
List *reconstruct();

/* Insert an integer into the heap. O(log n) */
void heap_add(int *heap, int size, int data);

/* Get the position with lowest cost+heuristc value. O(log n) */
int heap_get_min(int *heap, int size);


/*****************************************************************************
 *                         global variables                                  *
 *****************************************************************************/

/* Remember the goal while searching with A*. */
static int goal;

/* This array contains the actual cost for every visited node. */
static int *cost;
/* The array `f` contains the heuristic + cost of every node */
static int *f;

/* this is just for dubugging */
/*static int cost_goal;*/

/* This is the map representation in respect to the node weights. */
static int *weight;
static int height; // height of the map
static int width;  // width of the map

/* the heap containing nodes we have seen */
int *openset;

/* This array remembers the predecessor of every node. */
static int *came_from;

static int x1, y1;
static int x2, y2;

/*****************************************************************************
 *                         main functionality                                *
 *****************************************************************************/

/*
 * Call this method before starting the A* algorithm.
 */
void astar_init(const char *map, int _x1, int _y1, int _x2, int _y2) {
    x1 = _x1; y1 = _y1; x2 = _x2; y2 = _y2;

    /* memorize height and width of the map */
    height = MAP_WIDTH;
    width  = MAP_HEIGHT;
    
    /* allocate arrays, which we do not have to clean up */
    weight    = malloc(height*width * sizeof(int));    

    /* check if there was sufficent memory */    
    assert(weight != NULL);
        
    /* prepare datastructures */
    openset   = calloc(height*width, sizeof(int));
    cost      = calloc(height*width, sizeof(int));
    came_from = calloc(height*width, sizeof(int));
    f         = calloc(height*width, sizeof(int));    
    
    /* check for memory errors */
    assert(openset != NULL && cost != NULL && came_from != NULL
                && f != NULL);    
    
    /* convert the map into the weighted map */
    int i;
    for (i = 0; i < height*width; i++) {
        weight[i] = map_get_weight(map, i);
    }
}

/* calculate the heuristic of a node */
inline int heuristic(int n) {
    return 4 * max(abs(x(n)-x(goal)), abs(y(n)-y(goal)));
}

/*
 * the real algorithm!!!
 */ 
List* astar(int start, int _goal) {
    int size = 0;
    goal = _goal;
    
    /* add the starting point into the heap and start the A*-algorithm */
    heap_add(openset, size++, start);
    int pos = heap_get_min(openset, size--);
    while (pos != goal && pos != 0) {
        /* calculate all neighbors */
        int nbs[] = { 
            N(pos), S(pos), E(pos), W(pos),
            N(E(pos)), N(W(pos)), S(E(pos)), S(W(pos))
        };
        int i;
        int tentative_cost;
        /* insert new nodes into the heap and calculate their cost */
        for (i = 0; i < 8; i++) {
            if (is_in_map(nbs[i])) {
                   tentative_cost = cost[pos] + weight[nbs[i]];
                if (tentative_cost < cost[nbs[i]] || 
                        (cost[nbs[i]] == 0 && nbs[i] != start)) {
                    cost[nbs[i]] = tentative_cost;
                    f[nbs[i]]    = tentative_cost + heuristic(nbs[i]);
                    came_from[nbs[i]] = pos;
                    heap_add(openset, size++, nbs[i]);
                }
            }
        }
        pos = heap_get_min(openset, size--);
    }

    return reconstruct();
}

/*****************************************************************************
 *                          helping functions                                *
 *****************************************************************************/

/* swap two positions in an array */
inline void swap(int *heap, int i, int j) {
    int n = heap[i];
    heap[i] = heap[j];
    heap[j] = n;
}

/* repair the heap property of a node assuming all other nodes are OK. */
void heapify(int *heap, int size, int pos) {
    int min = pos;
    int left = left(pos);
    int right = right(pos);
    if (left < size && f[heap[left]] < f[heap[min]])
        min = left;
    if (right < size && f[heap[right]] < f[heap[min]])
        min = right;
    if (min != pos) {
        swap(heap, min, pos);
        heapify(heap, size, min);
    }
}

/* push a new element into the priorityqueue */
void heap_add(int *heap, int size, int data) {
    /* insert data at the end of the queue */
    heap[size] = data;

    /* swap with parent as int as we need to do so */
    int i = size;
    int p = parent(i);
    while (f[heap[p]] > f[heap[i]]) {
        swap(heap, p, i);
        i = p;
        p = parent(p);
    }
}

/* get minimum from heap */
int heap_get_min(int *heap, int size) {
    int min = heap[0];
    heap[0] = heap[--size];
    heapify(heap, size, 0);
    return min;
}

/* reconstruct the path on which we found the goal in the A* algorithm */
List *reconstruct() {
    List *path  = list_add(NULL, goal);
    int prev = came_from[goal];
    while (prev != 0) {
        path = list_add(path, prev);
        prev = came_from[prev];
    }
    return path;
}

/*****************************************************************************
 *                           user interface                                  *
 *****************************************************************************/

#define FILENAME "server1.csv"

#define start_x 0
#define start_y 1
#define goal_x  2
#define goal_y  3

#define x1 0
#define y1 1
#define x2 2
#define y2 3

typedef struct {

    const char *filename;
    int range[4];
    int coords[4];
    
} properties_t;


/*
 * Do some initital work. Check for arguments and try to open the csv file.
 */
int application_init(int argc, char **argv, properties_t *p) {
    if (argc < 5) {
        printf("please use the following syntax:\n");
        printf("%s [filename] [<x1> <y1> <x2> <y2>] <sx> <sy> <gx> <gy>\n",
                argv[0]);
        return -1;
    }
    
    memset(p, 0, sizeof(properties_t));
    
    int k = 1;
    if (argc == 6 || argc == 10) {
        p->filename = argv[k++];
    } else {
        p->filename = FILENAME;
    }
    
    if (argc == 10 || argc == 9) {
        p->range[x1] = atoi(argv[k++]);
        p->range[y1] = atoi(argv[k++]);
        p->range[x2] = atoi(argv[k++]);
        p->range[y2] = atoi(argv[k++]);

    } else {
        p->range[x1] = 1;
        p->range[y1] = 1;
        p->range[x2] = 2000;
        p->range[y2] = 2000;
    }
    
    p->coords[start_x] = atoi(argv[k++]);
    p->coords[start_y] = atoi(argv[k++]);
    p->coords[goal_x] = atoi(argv[k++]);
    p->coords[goal_y] = atoi(argv[k++]);
    
    /* try to get the map from a file */
    char *map;
    map = map_from_csv(p->filename);
    if (map == NULL) {
        printf("Could not open '%s': %s\n", p->filename, strerror(errno));
        return -1;
    }
    astar_init(map, p->range[x1], p->range[y1], p->range[x2], p->range[y2]);
    free(map);
    
    return 0;
}

/*
 * prints the path into the command line using the following format:
 * '<path>:<cost>\n'
 */
void print_path(List *path) {
    int i = 0;
    int p;
    /* iterate throughout the list and compare with successor. */
    List *node = path;
    List *next = path->next; 
    while (next != NULL) {
        p = node->data;
        int nbs[] = { 
            S(W(p)), S(p), S(E(p)), W(p), -1,
            E(p), N(W(p)), N(p), N(E(p))
        };
        for (i = 0; nbs[i] != next->data && i < 9; i++);
        assert(i < 9);
        printf("%d", i+1);
        node = next;
        next = next->next;
    }
    printf(":%d\n", cost[goal]);
}

int main(int argc, char *argv[]) {
    properties_t p;
    clock_t start, end;
    start = clock() / (CLOCKS_PER_SEC / 1000);
    
    /* parse arguments from command line */
    if (application_init(argc, argv, &p) == -1) {
        return -1;
    }
    
    /* run the astar algorithm */
    List *path = astar(
            pos(p.coords[start_x], p.coords[start_y]), 
            pos(p.coords[goal_x], p.coords[goal_y])
    );            
    print_path(path);
    list_delete(path);

    end = clock() / (CLOCKS_PER_SEC / 1000);
    printf("%d ms\n", (int)(end - start));
    return 0;
}
